#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include <sstream>
using namespace std;

#define PI 3.1415926

std::string p_base_stabilized_frame_;
std::string p_base_frame_;
std::string p_middle_frame_;
tf::TransformBroadcaster* tfB_;
tf::StampedTransform transform_;
tf::Quaternion tmp_;

#ifndef TF_MATRIX3x3_H
typedef btScalar tfScalar;
namespace tf {typedef btMatrix3x3 Matrix3x3;}
#endif

void aprilCallback(const geometry_msgs::PoseStamped& msg_stamped) {
	geometry_msgs::Pose msg = msg_stamped.pose;
	// find middle frame
	tmp_.setRPY(0, 0, 0);
	transform_.setRotation(tmp_);
	transform_.getOrigin().setX(-msg.position.x);
	transform_.getOrigin().setY(msg.position.y);
	transform_.getOrigin().setZ(msg.position.z);

	transform_.frame_id_ = p_middle_frame_;
	transform_.child_frame_id_ = p_base_frame_;
	//transform_.stamp_ = msg.header.stamp;

	tfB_->sendTransform(transform_);

	// find final frame
	tf::quaternionMsgToTF(msg.orientation, tmp_);
	tfScalar yaw, pitch, roll;
	tf::Matrix3x3(tmp_).getRPY(pitch, yaw, roll);

	ROS_INFO("%s %lf %lf %lf %lf %lf %lf", msg_stamped.header.frame_id.c_str(), msg.position.x, msg.position.y, msg.position.z, roll / PI * 180, pitch / PI * 180, yaw / PI * 180);
	// conversion to original
	pitch = pitch / std::abs(pitch) * PI - pitch;
	// turn yaw by pi
	yaw = yaw / std::abs(yaw) * PI - yaw;

	tmp_.setRPY(-roll, -pitch, -yaw);
	transform_.getOrigin().setX(0.0);
	transform_.getOrigin().setY(0.0);
	transform_.getOrigin().setZ(0.0);
	transform_.setRotation(tmp_);

	transform_.frame_id_ = p_base_stabilized_frame_;
	transform_.child_frame_id_ = p_middle_frame_;

	//transform_.stamp_ = msg.header.stamp;
	tfB_->sendTransform(transform_);

	// go back to get camera
	//
	//
	tmp_.setRPY(0, 0, 0);
	transform_.setRotation(tmp_);
	transform_.getOrigin().setX(msg.position.x);
	transform_.getOrigin().setY(-msg.position.y);
	transform_.getOrigin().setZ(-msg.position.z);

	transform_.frame_id_ = "camera";
	transform_.child_frame_id_ = "tag_t";
	//transform_.stamp_ = msg.header.stamp;

	tfB_->sendTransform(transform_);

	// find final frame
	tf::quaternionMsgToTF(msg.orientation, tmp_);
	tf::Matrix3x3(tmp_).getRPY(pitch, yaw, roll);

	//ROS_INFO("%d %lf %lf %lf %lf %lf %lf", msg_stamped.header.seq, msg.position.x, msg.position.y, msg.position.z, roll / PI * 180, pitch / PI * 180, yaw / PI * 180);
	// conversion to original
	pitch = pitch / std::abs(pitch) * PI - pitch;
	// turn yaw by pi
	yaw = yaw / std::abs(yaw) * PI - yaw;

	tmp_.setRPY(-roll, -pitch, yaw);
	transform_.getOrigin().setX(0.0);
	transform_.getOrigin().setY(0.0);
	transform_.getOrigin().setZ(0.0);
	transform_.setRotation(tmp_);

	transform_.frame_id_ = "tag_t";
	transform_.child_frame_id_ = "tag";

	//transform_.stamp_ = msg.header.stamp;
	tfB_->sendTransform(transform_);

	//******************
	//
	//
	tmp_.setRPY(0, 0, 0);
	transform_.setRotation(tmp_);
	transform_.getOrigin().setX(-msg.position.x);
	transform_.getOrigin().setY(msg.position.y);
	transform_.getOrigin().setZ(msg.position.z);

	transform_.frame_id_ = "tag_t1";
	transform_.child_frame_id_ = "camera1";
	//transform_.stamp_ = msg.header.stamp;

	tfB_->sendTransform(transform_);

	// find final frame
	tf::quaternionMsgToTF(msg.orientation, tmp_);
	tf::Matrix3x3(tmp_).getRPY(pitch, yaw, roll);

	//ROS_INFO("%d %lf %lf %lf %lf %lf %lf", msg_stamped.header.seq, msg.position.x, msg.position.y, msg.position.z, roll / PI * 180, pitch / PI * 180, yaw / PI * 180);
	// conversion to original
	pitch = pitch / std::abs(pitch) * PI - pitch;
	// turn yaw by pi
	yaw = yaw / std::abs(yaw) * PI - yaw;

	tmp_.setRPY(-roll, -pitch, -yaw);
	transform_.getOrigin().setX(0.0);
	transform_.getOrigin().setY(0.0);
	transform_.getOrigin().setZ(0.0);
	transform_.setRotation(tmp_);

	transform_.frame_id_ = "tag1";
	transform_.child_frame_id_ = "tag_t1";

	//transform_.stamp_ = msg.header.stamp;
	tfB_->sendTransform(transform_);
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "april_visualization");

	ros::NodeHandle n;
	ros::NodeHandle pn("~");

	pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string(
			"base_stabilized"));
	pn.param("base_frame", p_base_frame_, std::string("base_frame"));
	pn.param("tag_translation", p_middle_frame_, std::string("tag_translation"));
	tfB_ = new tf::TransformBroadcaster();
	ros::Subscriber april_sub = n.subscribe("april_pose", 10, aprilCallback);

	ros::spin();

	delete tfB_;

	return 0;
}
